% main function
% kneed biped, AMBER model, with torso

%angle convention
%q0 - angle the stance calf makes with the vertical, counterclockwise is positive
%q1 - relative stance knee angle,  counterclockwise is positive
%q2 - relative hip angle,          clockwise is positive
%q3 - relative swing knee angle,   counterclockwise is positive
% at q = (0,0,0,0) the biped is standing straight up, with locked knees

% clc;clear all;
clear tp tp0 yact ydes p data t u U tp Pow
addpath('./build_torso')
addpath('./wrappers_torso')
%Simulation variables
g = 981/100;
numsteps = 10;
tf = 0;
data = cell(numsteps,1);

% Parameters
umax = 8.9;
    
load('a_opt');
load('x_opt');
ic = x_opt;
a = a_opt;
ep = 10; %a(1,5);



%%%Variables used to store output data
global tp tp0 yact ydes p U umax Pow

tp0 = 0;
tp = [];
yact = [];
ydes = [];
U = [];
Pow = [];

%%%%%%(Comment this line if your initial condition is post-impact%%%%%
ic = resetFunc(ic);
ndof = length(ic)/2;

%Initial position of the hip

p = phip_sca(ic);



%Initialize animation plot
figure(2);
ramp = plot([-1 10], [0 0], 'b:');
px=0;
py=0;
hold on;
legs = plot([0 0], 'k');
hold off;
axis([-1 5 -1 1.2]);%

        
%plot initial condition
% pos = jpos_mat(ic);
%         phipz = pos(1,3)-p(1)-px;
%         set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
%             'erasemode', 'normal');
%         axis equal;
%         hold on;
%         drawnow;
        
%         pause(1000);

%%

for i=1:numsteps
    sol = ode23s(@(t,x) f1_vector(t,x,a,ep), [0 1.00], ic, ...
        odeset('Events', @(t,x) eventfcn(t,x,a), 'MaxStep', 1e-2));
    
    T = tf + sol.x;
    data{i} = [T; sol.y];
    ic = resetFunc(sol.y(:,end));
    tf = tf + sol.x(end);
    tp0 = tf;
    
    F = [sol.x; sol.y];
    [m, k] = size(data{i});    
    qo = cat(2, data{:});
    
    %
    for n=1:k
        % position
        x = [px;py;sol.y(:,n)];
        njoints = length(jpos_mat(sol.y(1:ndof,n)));
        pos = jpos_mat(sol.y(1:ndof,n))+...
            [px*ones(1,njoints); py*ones(1,njoints)];
        phipz = pos(1,3)-p(1)-px;
        set(legs, 'XData', pos(1,:), 'YData', pos(2,:), ...
            'erasemode', 'normal');
        axis equal;
        hold on;
        drawnow;
    end
    %update positions
    px = pos(1,end);
    py = pos(2,end);
    %update initial position of the hip
    ph = pos(1,3)-px; 
    p = ph;
end


%%


q=cat(2,data{:});

figure(1); clf;
subplot(2,1,1);
plot(q(1,:),q(2:6,:));
legend('Location','EastOutside',{'\theta_{sf}','\theta_{sk}','\theta_{ship}','\theta_{nship}','\theta_{nsk}'});
subplot(2,1,2);
plot(q(2,:),q(7,:), q(3,:),q(8,:), q(4,:),q(9,:),q(5,:),q(10,:),q(6,:),q(11,:));
legend('Location','EastOutside',{'\theta_{sf}','\theta_{sk}','\theta_{ship}','\theta_{nship}','\theta_{nsk}'});



for i = 1:length(q)
    t(i) = q(1,i);
    g(i) = h_sca(q(2:end,i));
end

disp('Plotting virtual outputs...');
figure(4); clf;
plot(tp, yact(1), 'k-', ...
    tp, yact(2), 'k:', ...
    tp, yact(3), 'k-.', ...
    tp, yact(4), 'b--', ...
    tp, yact(5), 'b-.', ...
    tp, ydes(1), 'r-', ...
    tp, ydes(2), 'r:', ...    
    tp, ydes(3), 'r-.', ...
    tp, ydes(4), 'c--', ...
    tp, ydes(5), 'c-.');
title('ya vs yd')
xlabel('time (s)');
ylabel('desired vs. actual')
legend('Location','EastOutside', {...
    'y_1 actual', ...
    'y_2 actual', ...
    'y_3 actual', ...
    'y_4 actual', ...
    'y_5 actual', ...
    'y_1 desired', ...
    'y_2 desired', ...
    'y_3 desired', ...
    'y_4 desired', ...
    'y_5 desired', ...
    'guard'});


%Must be only over one step to be accurate

%%
% 2) Limit the average torque over a step to 1.95 N-m ( this is to limit heat dissipation through winding resistance).
% 3) Limit the average power over a step to 8.61 W (this is to limit overall heat dissipation)
% 4) Limit the maximum torque to 8.9 N-m (the motor cannot provide more than this).
% 5) Limit the maximum speed to 6.54 rad/s ( the brushes will not connect well beyond this speed)

clear tstep qstep
qstep = data{1};
tstep = qstep(1,:);
[scotu,scotp,Ustep] = scot(qstep,a);

figure(6); clf;
% subplot(2,1,1);
% plot(tp,U,tp,AvgU*ones(1,length(tp)),'ko',tp,AvgP*ones(1,length(tp)),'ks')
plot(tstep,Ustep,...
    tstep,8.9*ones(1,length(tstep)),'k+',tstep,-8.9*ones(1,length(tstep)),'k+',...
    tstep,scotu*ones(1,length(tstep)),'m+',tstep,scotp*ones(1,length(tstep)),'m-')
title('Torque Outputs')
xlabel('time (s)');
ylabel('Torque (NM)')
legend('Location','EastOutside',{'\theta_{sf} torque','\theta_{sk} torque','\theta_{ship} torque','\theta_{nship} torque','\theta_{nsk} torque',...
    'max torque','min torque','u^2 norm','SCOT'});
% subplot(2,1,2);
% plot(tp,Pow,tp,AvgP*ones(1,length(tp)))
% title('Power Outputs')
% xlabel('time (s)');
% ylabel('Power')
% legend('Location','EastOutside',{'\theta_{sf} power','\theta_{sk} power','\theta_{hip} power','\theta_{nsk} power'});

%%

%%Output data to be tracked
%%%

outputdata = 0

if outputdata == 1



qtemp = data{1};
qtime = [qtemp(1,:),qtemp(1,:)+qtemp(1,end)];
qknee_left = [qtemp(3,:),qtemp(6,:)];
qknee_right = [qtemp(6,:),qtemp(3,:)];
qhip_left = [qtemp(4,:),qtemp(5,:)];
qhip_right = [qtemp(5,:),qtemp(4,:)];
% qankle = [qtemp(2,:),pi/2-(qtemp(4,:)-qtemp(2,:)+(pi-qtemp(3,:))-(pi-qtemp(5,:)))];
% qkneevel = [qtemp(9,:),qtemp(7,:)];
% qkneetorq = [U(4,:),U(2,:)];

figure(8); clf;
plot(qtime,qknee_left,qtime,qhip_left,qtime,qknee_right,qtime,qhip_right)
legend('Location','EastOutside',{'Left knee','Left hip','Right knee','Right hip'})

% 
% figure(7); clf;
% plot(qtime,qknee,qtime,qkneevel,[tp, tp+tp(end)],qkneetorq)
% title('Knee behavior')
% xlabel('time (s)');
% legend('Location','EastOutside',{'position','velocity','torque'})

addpath('./ExperimentData')
save('./ExperimentData/kneetraj.mat','qtime','qknee_left','qhip_left','qhip_right','qknee_right')
save ./ExperimentData/knee_left_traj qknee_left -ascii -double -tabs
save ./ExperimentData/hip_left_traj qhip_left -ascii -double -tabs
save ./ExperimentData/knee_right_traj qknee_right -ascii -double -tabs
save ./ExperimentData/hip_right_traj qhip_right -ascii -double -tabs

end
